/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2017-2020, Waterplus http://www.6-robot.com
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the WaterPlus nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  FOOTPRINTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
/* @author Zhang Wanjie                                             */

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Pose.h>
#include <sensor_msgs/JointState.h>
#include <tf/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <dynamic_reconfigure/server.h>
#include <visualization_msgs/Marker.h>
#include "wpv4_behaviors/mani_planConfig.h"
#include "SimpleArm.h"

static geometry_msgs::Pose target_pose;             // 获取规划目标
static ros::Publisher result_pub;                   // 执行结果发布
static std_msgs::String result_msg;                 // 执行结果消息
static tf::StampedTransform transform;
static visualization_msgs::Marker plan_marker;
static ros::Publisher plan_marker_pub;
static ros::Publisher joint_ctrl_pub;
static CSimpleArm simple_arm;

#define STEP_READY        0
#define STEP_PLAN         1
#define STEP_EXEC         2
int step = STEP_READY;

bool new_plan = false;

void PlanCallback(const geometry_msgs::Pose::ConstPtr& msg)
{
    if(step != STEP_READY)
    {
        result_msg.data = "buzy";
        result_pub.publish(result_msg);
        return;
    }
    // 规划目标
    target_pose = *msg;
    ROS_WARN("[PlanCallback] x = %.2f y= %.2f ,z= %.2f " ,target_pose.position.x, target_pose.position.y, target_pose.position.z);
    new_plan = true;
}

void CfgManiPlanCallback(wpv4_behaviors::mani_planConfig &config, uint32_t level) 
{
    if(step != STEP_READY)
    {
        return;
    }
    target_pose.position.x = config.target_x;
    target_pose.position.y = config.target_y;
    target_pose.position.z = config.target_z;
    ROS_WARN("动态设置规划目标 (%.2f , %.2f , %.2f)", target_pose.position.x, target_pose.position.y, target_pose.position.z);
    // 机械臂末端朝向
    tf::Quaternion quat;
    // 手爪初始姿态（全部为0为朝上）,函数三个参数分别为滚转,俯仰和偏转角,单位为弧度
    quat.setRPY(config.target_roll, config.target_pitch, config.target_yaw);
    // 将欧拉角旋转量转换成四元数表达
    transform.setRotation(quat);
    target_pose.orientation.x= transform.getRotation().getX();
    target_pose.orientation.y = transform.getRotation().getY();
    target_pose.orientation.z = transform.getRotation().getZ();
    target_pose.orientation.w = transform.getRotation().getW();
    new_plan = true;
}

int main(int argc, char** argv)
{
    setlocale(LC_ALL,"");
    ros::init(argc, argv, "wpv4_simple_plan");

    ros::NodeHandle nh;
    ros::Subscriber plan_sub = nh.subscribe("/wpv4/mani_plan", 1 , PlanCallback);
    result_pub =  nh.advertise<std_msgs::String>("/wpv4/plan_result", 10);
    plan_marker_pub = nh.advertise<visualization_msgs::Marker>("/wpv4/plan_marker", 10);
    joint_ctrl_pub = nh.advertise<sensor_msgs::JointState>("wpm2/joint_ctrl_degree", 10);

    sensor_msgs::JointState ctrl_msg;
    ctrl_msg.name.resize(7);
    ctrl_msg.position.resize(7);
    ctrl_msg.velocity.resize(7);
    //关节角度
    ctrl_msg.position[0] = 0;
    ctrl_msg.position[1] = 0;
    ctrl_msg.position[2] = 90;
    ctrl_msg.position[3] = 0;
    ctrl_msg.position[4] = -90;
    ctrl_msg.position[5] = 0;
    // ctrl_msg.position[6] = 35000; //手爪闭合
    ctrl_msg.position[6] = 10000; //手爪张开
    //关节运动速度
    ctrl_msg.velocity[0] = 1000;
    ctrl_msg.velocity[1] = 1000;
    ctrl_msg.velocity[2] = 1000;
    ctrl_msg.velocity[3] = 1000;
    ctrl_msg.velocity[4] = 1000;
    ctrl_msg.velocity[5] = 1000;
    ctrl_msg.velocity[6] = 1000;

    dynamic_reconfigure::Server<wpv4_behaviors::mani_planConfig> mani_plan_server;
    dynamic_reconfigure::Server<wpv4_behaviors::mani_planConfig>::CallbackType mani_plan_func;

    mani_plan_func = boost::bind(&CfgManiPlanCallback, _1, _2);
    mani_plan_server.setCallback(mani_plan_func);


    ros::Rate r(30);
    while(nh.ok())
    {
        if(new_plan == true)
        {
            // 发送进度消息
            result_msg.data = "plan";
            result_pub.publish(result_msg);
            step = STEP_PLAN;
            // 在rviz里显示
            plan_marker.header.frame_id = "base_footprint";
            plan_marker.ns = "arrow_place";
            plan_marker.action = visualization_msgs::Marker::ADD;
            plan_marker.id = 1;
            plan_marker.type = visualization_msgs::Marker::ARROW;
            plan_marker.scale.x = 0.2;
            plan_marker.scale.y = 0.08;
            plan_marker.scale.z = 0.08;
            plan_marker.color.r = 1.0;
            plan_marker.color.g = 0;
            plan_marker.color.b = 1.0;
            plan_marker.color.a = 0.5;
            plan_marker.pose = target_pose;
            plan_marker.pose.position.y = 0;
            tf2::Quaternion quat_new;
            quat_new.setRPY(0, 0, 0);
            plan_marker.pose.orientation = tf2::toMsg(quat_new);
            plan_marker_pub.publish(plan_marker);
            // 简单规划
            double x = target_pose.position.x - 0.24;
            double z = target_pose.position.z - /*0.16*/0.26 - 0.1585 - 0.0825; //base->joint1 , joint1->joint2 , joint2->joint3
            ROS_INFO("规划的机械臂本地坐标为 ( %.2f , %.2f )",x,z);
            bool res = simple_arm.SetTargetPos(x , z);
            if( res == true)
            {
                ROS_INFO("规划结果 关节角: [0]=%.2f  [1]=%.2f [2]=%.2f",simple_arm.JointAngle[0] ,simple_arm.JointAngle[1] ,simple_arm.JointAngle[2]);
                ctrl_msg.position[1] = simple_arm.JointAngle[0];
                ctrl_msg.position[3] = simple_arm.JointAngle[1];
                ctrl_msg.position[5] = simple_arm.JointAngle[2];
                joint_ctrl_pub.publish(ctrl_msg);
            }
            else
            {
                ROS_WARN(" 机械臂规划失败！ 请检查坐标数值……");
            }
            new_plan = false;
            step = STEP_READY;
        }
        ros::spinOnce();
        r.sleep();
    }
    return 0;
}
